/* gm_robot.c -- Representação de um robo
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <unistd.h>

#include "gm_utils.h"
#include "gm_robot.h"

/* enum */
enum read_who
{
    READ_WORLD,
    READ_FIELD
};

/* Static functions */
static int gm_robot_make_socket( void );
static void gm_robot_fill_sockaddr( gm_robot_t * self, const char *hostname, uint16_t port );
static void gm_robot_connect( gm_robot_t * self );
static void gm_robot_build_msg( gm_robot_t * self, enum msg_cmd msg_type );
static void gm_robot_send( gm_robot_t * self );
static void gm_robot_parse_field( gm_robot_t * self, const char * buffer );
static void gm_robot_wait_response_server( gm_robot_t * self, enum read_who who );
static void gm_robot_parse_world( gm_robot_t * self, const char * buffer );

/** Cria um novo robô.
 *
 * @param id Código inteiro do robô.
 *
 * @param end[] Endereço do robô.
 *
 * @param *serial_path Caminho para a serial.
 *
 * @param *server Ponteiro para o servidor, de onde as informações serão
 * retiradas.
 *
 * @param *strategy_func Ponteiro para a função de estratégia.
 *
 * @return Ponteiro para um novo robô.
 */
gm_robot_t *
gm_robot_new( const int id,
	      const char end[3],
	      const char * addr,
	      const int port )
{
    gm_robot_t * self = NULL;

    self = XMALLOC( gm_robot_t, 1);

    self->id = id;
    self->end[0] = end[0];
    self->end[1] = end[1];
    self->end[2] = end[2];

    self->sock = gm_robot_make_socket();
    gm_robot_fill_sockaddr( self,
			    addr,
			    port );
    gm_robot_connect( self );
    
    /* Criando o vetor de informações */
    self->info = XMALLOC( float, LENGTH_FIELD_VECTOR );
    self->info_size = LENGTH_FIELD_VECTOR;
    memset( self->info, 0, LENGTH_FIELD_VECTOR );

    /* Criando o vetor do mundo */
    self->world = XMALLOC( float, LENGTH_WORLD_VECTOR );
    self->world_size = LENGTH_WORLD_VECTOR;
    memset( self->world, 0, LENGTH_WORLD_VECTOR );

    /* Criando o vetor do campo */
    self->field = XMALLOC( float, LENGTH_FIELD_VECTOR );
    self->field_size = LENGTH_FIELD_VECTOR;
    memset( self->field, 0, LENGTH_FIELD_VECTOR );

    /* Zerando velocidades */
    self->vx = 0;
    self->vy = 0;
    self->va = 0;

    /* Drible desligado */
    self->dribble_enable = 0;

    return self;
}

void
gm_robot_destroy( gm_robot_t * self )
{
    XFREE( self->field );
    XFREE( self->info );
    XFREE( self->world );

    close( self->sock );
    
    XFREE( self );
}

static void
gm_robot_connect( gm_robot_t * self )
{
    if (0 > connect (self->sock,
		     (struct sockaddr *) &(self->servername),
		     sizeof (self->servername)))
    {
	xerror_fatal("Não foi possível conectar no servidor");
    }
}

static void
gm_robot_fill_sockaddr( gm_robot_t * self,
			const char *hostname,
			uint16_t port )
{
    struct hostent *hostinfo;
     
    self->servername.sin_family = AF_INET;
    self->servername.sin_port = htons (port);
    hostinfo = gethostbyname (hostname);
    if (hostinfo == NULL)
    {
	xerror_error("Host desconhecido %s", hostname);
	exit (EXIT_FAILURE);
    }
    self->servername.sin_addr = *(struct in_addr *) hostinfo->h_addr;

}

static int
gm_robot_make_socket( void )
{
    int sock;
     
    /* Create the socket. */
    sock = socket (PF_INET, SOCK_STREAM, 0);
    if (sock < 0)
	xerror_fatal("Não consegui criar um socket.");
     
    return sock;
}

void
gm_robot_say_hello( gm_robot_t * self )
{

    gm_robot_build_msg( self,
			GM_MSG_CMD_SAY_HELLO );
    gm_robot_send( self );
}

static void
gm_robot_send( gm_robot_t * self )
{
    int nbytes;
     
    nbytes = write ( self->sock,
		     self->msg,
		     MAX_LENGTH_MSG);
    if (nbytes < 0)
    {
	xerror_error("A mensagem não pode ser enviada.");
    }
}

static void
gm_robot_build_msg( gm_robot_t * self,
		    enum msg_cmd msg_type )
{
    char info[32];

    /* Primeiro, "limpamos" a mensagem */
    self->msg[0] = '\0';
    
    /* Caracterizando a mensagem como da IA */
    gm_utils_int2str( info, GM_MSG_IA, 32);
    strcat( self->msg, info );
    strcat( self->msg, "|" );

    /* Colocando o ID do robô */
    gm_utils_int2str( info, self->id, 32);
    strcat( self->msg, info );
    strcat( self->msg, "|" );

    switch( msg_type )
    {
    case GM_MSG_CMD_READ_WORLD:
    {
	gm_utils_int2str( info, GM_MSG_CMD_READ_WORLD, 32);
	strcat( self->msg, info );
	strcat( self->msg, "|" );

	break;
    }
    case GM_MSG_CMD_READ:
    {
	gm_utils_int2str( info, GM_MSG_CMD_READ, 32);
	strcat( self->msg, info );
	strcat( self->msg, "|" );
	
	break;
    }
    case GM_MSG_CMD_MOVE:
    {
	gm_utils_int2str( info, GM_MSG_CMD_MOVE, 32);
	strcat( self->msg, info );
	strcat( self->msg, "|" );

	gm_utils_float2str( info, self->vx, 32);
	strcat( self->msg, info );
	strcat( self->msg, "|" );

	gm_utils_float2str( info, self->vy, 32);
	strcat( self->msg, info );
	strcat( self->msg, "|" );

	gm_utils_float2str( info, self->va, 32);
	strcat( self->msg, info );
	strcat( self->msg, "|" );
	
	break;
    }
    case GM_MSG_CMD_DRIBBLE_ENABLE:
    {
	gm_utils_int2str( info, GM_MSG_CMD_DRIBBLE_ENABLE, 32);
	strcat( self->msg, info );
	strcat( self->msg, "|" );

	gm_utils_int2str( info, self->dribble_enable, 32);
	strcat( self->msg, info );
	strcat( self->msg, "|" );
	break;
    }
    case GM_MSG_CMD_KICK:
    {
	gm_utils_int2str( info, GM_MSG_CMD_KICK, 32);
	strcat( self->msg, info );
	strcat( self->msg, "|" );

	gm_utils_int2str( info, self->kick_lvl, 32);
	strcat( self->msg, info );
	strcat( self->msg, "|" );
	
	break;
    }
    case GM_MSG_CMD_SAY_HELLO:
    {
	gm_utils_int2str( info, GM_MSG_CMD_SAY_HELLO, 32);
	strcat( self->msg, info );
	strcat( self->msg, "|" );
	
	break;
    }
    }
}

/** Seta a velocidade frontal, lateral e de rotação do robô.
 *
 *
 * @param *self Objeto gm_robot_t.
 *
 * @param vx Velocidade frontal em m/s.
 *
 * @param vy Velocidade lateral em m/s.
 *
 * @param va Velocidade de rotação em rad/s.
 */
void
gm_robot_set_vel( gm_robot_t * self, float vx, float vy, float va )
{
    self->vx = vx;
    self->vy = vy;
    self->va = va;

    gm_robot_build_msg( self,
			GM_MSG_CMD_MOVE );
    gm_robot_send( self );    
}


static void
gm_robot_parse_field( gm_robot_t * self, const char * buffer )
{
    float info = 0;
    int iter = 0;
    int i = 0;

    for( i = 0 ; i < self->field_size ; i++ )
    {
	info = gm_utils_vec_get_next_float( buffer, &iter );
	self->field[i] = info;
    }
    
}

static void
gm_robot_parse_world( gm_robot_t * self, const char * buffer )
{
    float info = 0;
    int iter = 0;
    int i = 0;

    for( i = 0 ; i < self->world_size ; i++ )
    {
	info = gm_utils_vec_get_next_float( buffer, &iter );
	self->world[i] = info;
    }
    
}

static void
gm_robot_wait_response_server( gm_robot_t * self, enum read_who who )
{
    int nbytes = 0;
    char buffer[MAX_LENGTH_MSG];

    nbytes = read (self->sock, buffer, MAX_LENGTH_MSG);

    if( nbytes < 0 )
    {
	xerror_error("robot: Não consegui ler a resposta do servidor");
    }

    switch (who)
    {
    case READ_WORLD:
    {
	gm_robot_parse_world( self, buffer );
	break;
    }
    case READ_FIELD:
    {
	gm_robot_parse_field( self, buffer );

	break;
    }
    }
}


void
gm_robot_read_world( gm_robot_t *self)
{
    gm_robot_build_msg( self,
			GM_MSG_CMD_READ_WORLD );
    gm_robot_send( self );

    gm_robot_wait_response_server( self, READ_WORLD );

}


/** Atualiza as informações de jogo no robô.
 *
 * Esta função acessa o servidor, logo, ela não volta instantaneamente.
 *
 * @param *self Objeto gm_robot_t.
 */
void
gm_robot_read( gm_robot_t * self )
{
    int i = 0;

    gm_robot_build_msg( self,
			GM_MSG_CMD_READ );
    gm_robot_send( self );

    gm_robot_wait_response_server( self, READ_FIELD );

    for( i = 0 ; i < LENGTH_FIELD_VECTOR ; i++ )
	self->info[i] = self->field[i];
}

/** Chuta.
 *
 * @param *self Objeto gm_robot_t.
 */
void
gm_robot_kick( gm_robot_t *self, const int nivel)
{
    self->kick_lvl = nivel;
    
    gm_robot_build_msg( self,
			GM_MSG_CMD_KICK );
    gm_robot_send( self );    
}

/** Liga ou desliga o sistema de chute.
 *
 * @param *self Objeto gm_robot_t.
 *
 * @param enable 1 para ligar, 0 para desligar.
 */
void
gm_robot_dribble_enable( gm_robot_t * self, const int enable )
{
    if( enable )
	self->dribble_enable = 1;
    else
	self->dribble_enable = 0;

    gm_robot_build_msg( self,
			GM_MSG_CMD_DRIBBLE_ENABLE );
    gm_robot_send( self );    
}
